#include "ars_40x_msgs/msg/speed_information.hpp"
#include "ars_40x_msgs/msg/yaw_rate_information.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "tf2/exceptions.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
template <typename T, typename d> void xyz_set(T &obj, d x, d y, d z)
{
	obj.x = x;
	obj.y = y;
	obj.z = z;
	return;
}

template <typename T, typename d> void xyz_add(T &obj, d x, d y, d z)
{
	obj.x += x;
	obj.y += y;
	obj.z += z;
	return;
}

template <typename T, typename d> void xyz_sub(T &obj, d x, d y, d z)
{
	obj.x -= x;
	obj.y -= y;
	obj.z -= z;
	return;
}

class posepub : public rclcpp::Node
{
  private:
	rclcpp::Publisher<ars_40x_msgs::msg::SpeedInformation>::SharedPtr speed_pub;
	rclcpp::Publisher<ars_40x_msgs::msg::YawRateInformation>::SharedPtr yawrate_pub;
	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub;
	double speed;
	double yaw, roll, pitch;
	double yawrate;
	geometry_msgs::msg::Pose pose;
	geometry_msgs::msg::TransformStamped gt;
	geometry_msgs::msg::Vector3 vel;
	tf2_ros::Buffer::SharedPtr tf_buf;
	geometry_msgs::msg::PointStamped gzero;
	std::unique_ptr<tf2_ros::TransformBroadcaster> tf_pub;
	std::shared_ptr<tf2_ros::TransformListener> tf_sub;
	geometry_msgs::msg::Transform radar_trans;
	rclcpp::TimerBase::SharedPtr timer;

	// update imu position, orientation, velocity
	void imu_callback(sensor_msgs::msg::Imu msg)
	{
		static int calibrate = 200;
		static auto lastsample = rclcpp::Clock().now();
		auto thissample = rclcpp::Clock().now();
		double dt = (thissample - lastsample).seconds();
		lastsample = thissample;
		if (dt < 0.001)
		{
			return;
		}
		
		if (calibrate > 0)
		{
			gt.header.frame_id = "map";
			gt.header.stamp = rclcpp::Clock().now();
			gt.child_frame_id = "gzero";
			gt.transform.rotation = msg.orientation;
			xyz_set(gt.transform.translation, 0, 0, 0);
			this->tf_pub->sendTransform(gt);
			this->gzero.header.frame_id = "gzero";
			this->gzero.header.stamp = rclcpp::Clock().now();
			xyz_set(this->gzero.point, msg.linear_acceleration.x, msg.linear_acceleration.y,
					msg.linear_acceleration.z);
			calibrate--;
			return;
		}

		double dvx, dvy, dvz;
		// update imu orientation
		this->pose.orientation = msg.orientation;
		// gravity
		geometry_msgs::msg::TransformStamped it;
		it.header.frame_id = "map";
		it.header.stamp = rclcpp::Clock().now();
		it.child_frame_id = "i";
		it.transform.rotation = msg.orientation;
		xyz_set(it.transform.translation, 0, 0, 0);
		this->tf_pub->sendTransform(it);
		gt.header.stamp = rclcpp::Clock().now();
		this->tf_pub->sendTransform(gt);
		this->gzero.header.stamp = rclcpp::Clock().now();
		auto igt = this->tf_buf->lookupTransform("i", "gzero", tf2::TimePointZero);
		auto g = this->tf_buf->transform(this->gzero, "i");
		xyz_sub(msg.linear_acceleration, g.point.x, g.point.y, g.point.z);
		RCLCPP_INFO(this->get_logger(), "gzero %lf %lf %lf", g.point.x, g.point.y, g.point.z);
		// speed increment
		dvx = msg.linear_acceleration.x * dt;
		dvy = msg.linear_acceleration.y * dt;
		dvz = msg.linear_acceleration.z * dt;
		// position increment
		this->pose.position.x += (this->vel.x + dvx / 2) * dt;
		this->pose.position.y += (this->vel.y + dvy / 2) * dt;
		this->pose.position.z += (this->vel.z + dvz / 2) * dt;
		// update speed
		this->vel.x += dvx;
		this->vel.y += dvy;
		this->vel.z += dvz;
		// publish imu tf
		geometry_msgs::msg::TransformStamped t;
		t.header.stamp = this->get_clock()->now();
		// imu to map
		t.header.frame_id = "map";
		t.child_frame_id = "imu";
		t.transform.translation.x = this->pose.position.x;
		t.transform.translation.y = this->pose.position.y;
		t.transform.translation.z = this->pose.position.z;
		t.transform.rotation = msg.orientation;
		this->tf_pub->sendTransform(t);
		// radar to imu
		t.header.frame_id = "imu";
		t.child_frame_id = "radar";
		t.transform = this->radar_trans;
		this->tf_pub->sendTransform(t);

		// update radar speed and yawrate
		// dummy pose struct
		geometry_msgs::msg::Pose pin, pout;
		pin.position.x = this->vel.x;
		pin.position.y = this->vel.y;
		pin.position.z = this->vel.z;
		pin.orientation = this->pose.orientation;
		// transform to map
		tf2::doTransform<geometry_msgs::msg::Pose>(pin, pout, t);
		// speed vector in x
		this->speed = pout.position.x;
		// calculate yawrate
		tf2::Quaternion q(pout.orientation.x, pout.orientation.y, pout.orientation.z,
						  pout.orientation.w);
		tf2::Matrix3x3 m(q);
		double yaw_, dyaw;
		m.getRPY(this->roll, this->pitch, yaw_);
		this->roll *= 180;
		this->pitch *= 180;
		yaw_ *= 180;
		dyaw = (yaw_ - yaw);
		dyaw = dyaw > 180 ? dyaw - 180 : (dyaw < -180 ? dyaw + 180 : dyaw);
		this->yawrate = dyaw / dt;
		this->yaw = yaw_;
		RCLCPP_INFO(this->get_logger(), "imu yaw %lf", this->yaw);
	}

	void timer_callback()
	{
		ars_40x_msgs::msg::SpeedInformation smsg;
		ars_40x_msgs::msg::YawRateInformation ymsg;
		smsg.speed = this->speed;
		this->speed_pub->publish(smsg);
		ymsg.yawrate = this->yawrate;
		this->yawrate_pub->publish(ymsg);
		RCLCPP_INFO(this->get_logger(), "published to radar: speed %lf, yawrate %lf",
					this->speed, this->yawrate);
	}

  public:
	posepub(std::string name) : Node(name)
	{
		this->declare_parameter("radar_trans_x", this->radar_trans.translation.x);
		this->declare_parameter("radar_trans_y", this->radar_trans.translation.y);
		this->declare_parameter("radar_trans_z", this->radar_trans.translation.z);
		this->declare_parameter("radar_rotat_x", this->radar_trans.rotation.x);
		this->declare_parameter("radar_rotat_y", this->radar_trans.rotation.y);
		this->declare_parameter("radar_rotat_z", this->radar_trans.rotation.z);
		this->declare_parameter("radar_rotat_w", this->radar_trans.rotation.w);
		this->get_parameter("radar_trans_x", this->radar_trans.translation.x);
		this->get_parameter("radar_trans_y", this->radar_trans.translation.y);
		this->get_parameter("radar_trans_z", this->radar_trans.translation.z);
		this->get_parameter("radar_rotat_x", this->radar_trans.rotation.x);
		this->get_parameter("radar_rotat_y", this->radar_trans.rotation.y);
		this->get_parameter("radar_rotat_z", this->radar_trans.rotation.z);
		this->get_parameter("radar_rotat_w", this->radar_trans.rotation.w);
		this->speed_pub =
			this->create_publisher<ars_40x_msgs::msg::SpeedInformation>("radar_speed", 1);
		this->yawrate_pub =
			this->create_publisher<ars_40x_msgs::msg::YawRateInformation>("radar_yawrate", 1);
		this->imu_sub = this->create_subscription<sensor_msgs::msg::Imu>(
			"Imu_data", 1, std::bind(&posepub::imu_callback, this, std::placeholders::_1));

		this->tf_pub = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
		this->tf_buf = std::make_unique<tf2_ros::Buffer>(this->get_clock());
		this->tf_sub = std::make_shared<tf2_ros::TransformListener>(*this->tf_buf);
		this->timer = this->create_wall_timer(std::chrono::milliseconds(100),
											  std::bind(&posepub::timer_callback, this));
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<posepub>("posepub");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
